/**
 * Puny objects! You are nothing next to the SUPERMAN ALGORITHM!
 * 
 * This algorithm beelines for the goal while completely ignoring any obstacles on the map
 * A test algorithm
 */

using System;
using System.Collections.Generic;
using System.Text;
using System.Collections;
using Microsoft.Xna.Framework;

namespace Autobot
{
    public enum DelayMS : int
    {
        SHORT = 15,
        MEDIUM = 25,
        LONG = 35
    }
    class Mulletgorithm : AlgorithmBase
    {
        // Static string to identify this class in the Algorithms class
        public static readonly new string Name = "Mulletgorithm";

        // Store a reference to the world objects
        public Mulletgorithm(VirtualMap mapReference, Robot robotReference, Goal goalReference)
        {
            map = mapReference;
            robot = robotReference;
            goal = goalReference;
            OldSensors = new float[3];
            OldSensors[0] = 0.0f;
            OldSensors[1] = 0.0f;
            OldSensors[2] = 0.0f;
        }

        // Degrees the goal can be off of the robot so that it will think of it as being in a straight line
        private const double StraightLineAngle = 1.00;
        private float[] OldSensors;

        public override AlgorithmResult GetNextResult()
        {
            // Get a new result ready to return
            AlgorithmResult NewResult = new AlgorithmResult();

            // If the robot isn't moving, tell it to increment its speed so it starts moving
            if (robot.GetVelocity() == 0.0f)
            {
                NewResult.AddCommand(CreateIncSpeedCommand(10,0));
                previousResults.Push(NewResult);
                return previousResults.Peek();
            }

            // If we reached the goal then stop the car
            if(goal.CheckGoalReached(robot.GetRealPosition()))
            {
                NewResult.AddCommand(CreateStopCommand(0));
                previousResults.Push(NewResult);
                return previousResults.Peek();
            }

            // Determine the angle the robot needs to go to reach the goal so the robot can turn left or right

            // Get the angle the robot is driving at, convert it to HUMAN terms,
            // Stupid XNA flipping the Y direction... argh
            Vector2 robotDir = robot.GetDirection();

            double robotAngle = Math.Atan2((double)robotDir.Y, (double)robotDir.X);

            // We want all the big angles
            // Robot is going up and to the right
            if ((robotDir.Y < 0) && (robotDir.X > 0))
                robotAngle = -robotAngle;
            // Robot is going up and to the left
            else if ((robotDir.Y < 0) && (robotDir.X < 0))
                robotAngle = -robotAngle;
            // Robot is going down and to the left
            else if ((robotDir.Y > 0) && (robotDir.X < 0))
                robotAngle = 2.0 * Math.PI - robotAngle;
            // Robot is goint down and to the right
            else if ((robotDir.Y > 0) && (robotDir.X > 0))
                robotAngle = 2.0 * Math.PI - robotAngle;

            Vector2 RobotPosition = robot.GetVirtualPosition();
            Vector2 GoalPosition = goal.GetVirtualPosition();

            Vector2 RobotToGoal = GoalPosition - RobotPosition;

            // Get the angle from the robot to the goal, convert it to HUMAN terms,
            // Stupid XNA flipping the Y direction... argh
            //RobotToGoal.Y = -RobotToGoal.Y;
            //RobotToGoal.X = -RobotToGoal.X;

            double RobotToGoalAngle = Math.Atan2((double)RobotToGoal.Y, (double)RobotToGoal.X);

            // If the goal is above and to the left of the robot
            if ((GoalPosition.Y < RobotPosition.Y ) && (GoalPosition.X < RobotPosition.X))
            {    
                RobotToGoalAngle = -RobotToGoalAngle;
            }
            // Goal above and to the right of the robot
            else if ((GoalPosition.Y < RobotPosition.Y) && (GoalPosition.X > RobotPosition.X))
            {
                RobotToGoalAngle = -RobotToGoalAngle;
            }
            // Goal below and to the left of robot
            else if ((GoalPosition.Y > RobotPosition.Y) && (GoalPosition.X < RobotPosition.X))
            {
                RobotToGoalAngle = 2*Math.PI - RobotToGoalAngle;
            }
            // Goal below and to the right of robot
            else if ((GoalPosition.Y > RobotPosition.Y) && (GoalPosition.X > RobotPosition.X))
            {
                RobotToGoalAngle = 2*Math.PI - RobotToGoalAngle;
            }

            // MessageBoxDrawer.Show("Robot Angle: " + robotAngle.ToString() + " RobotToGoal: " + RobotToGoalAngle.ToString());

            // Check the sensor data
            RobotData data = robot.GetDataCopy();
            float[] Sensors = data.sensors;

            // A threshold that might warrant change
            float ChangeThreshold = 0.7f;
            
            // A threshold that declares immediate change
            float CriticalThreshold = 0.4f;

            float DriveMotor = data.motors[(int)MotorID.DRIVE];

            // Check if only the left sensor is below Crit Threshold
            if ((Sensors[(int)SensorID.LEFT] < CriticalThreshold) && (Sensors[(int)SensorID.MIDDLE] > CriticalThreshold) && (Sensors[(int)SensorID.RIGHT] > CriticalThreshold))
            {
                // Going forward
                if (DriveMotor > (int)DriveMotorParams.STOP)
                {

                    // Do a little jig turn away from this sensor
                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, 0));
                    NewResult.AddCommand(CreateTurnRightCommand(SteeringMotorParams.MAX_LEFT, 0));
                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, (int)DelayMS.LONG));
                }
                   
            }
            // Check for only left sensor below Changing threshold
            else if ((Sensors[(int)SensorID.LEFT] < ChangeThreshold) && (Sensors[(int)SensorID.MIDDLE] > ChangeThreshold) && (Sensors[(int)SensorID.RIGHT] > ChangeThreshold))
            {
                // Going forward
                if (DriveMotor > (int)DriveMotorParams.STOP)
                {
                    // Check if the we are moving towards the obstacle
                    if (Sensors[(int)SensorID.LEFT] - OldSensors[(int)SensorID.LEFT] < 0.0f)
                    {
                        // Do a little jig turn away from this sensor
                        NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, 0));
                        NewResult.AddCommand(CreateTurnRightCommand(SteeringMotorParams.MAX_RIGHT, 0));
                        NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, (int)DelayMS.LONG));
                    }
                    else
                    {
                        // Head towards the goal if it is on the right side
                        NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, 0));
                        if (robotAngle < RobotToGoalAngle)
                        {
                            if (robotAngle > (RobotToGoalAngle - Math.PI))
                                NewResult.AddCommand(CreateTurnRightCommand(SteeringMotorParams.MAX_RIGHT, 0));
                        }
                        NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, (int)DelayMS.LONG));
                    }
                }
            }
            // Only middle sensor is below critical threshold
            else if ((Sensors[(int)SensorID.LEFT] > CriticalThreshold) && (Sensors[(int)SensorID.MIDDLE] < CriticalThreshold) && (Sensors[(int)SensorID.RIGHT] > CriticalThreshold))
            {
                // Going forward
                if (DriveMotor > (int)DriveMotorParams.STOP)
                {
                    Random random = new Random();
                    int dir = random.Next() % 2;

                    // So what happens now? our forward path is completely blocked. Back up and go around
                    NewResult.AddCommand(CreateSetDriveMotorCommand((int)DriveMotorParams.MIN,0));
                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP,0));

                    if (dir == 0)
                        NewResult.AddCommand(CreateTurnRightCommand(SteeringMotorParams.MAX_RIGHT,0));
                    else
                        NewResult.AddCommand(CreateTurnLeftCommand(SteeringMotorParams.MAX_LEFT,0));

                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, (int)DelayMS.LONG));
                    NewResult.AddCommand(CreateSetDriveMotorCommand((int)DriveMotorParams.MAX, (int)DelayMS.LONG));
                }
            }
            // Only middle sensor is below changing threshold
            else if ((Sensors[(int)SensorID.LEFT] > ChangeThreshold) && (Sensors[(int)SensorID.MIDDLE] < ChangeThreshold) && (Sensors[(int)SensorID.RIGHT] > ChangeThreshold))
            {
                // If we are heading right for the goal, then avoid the obstacle
                if (Math.Abs(robotAngle - RobotToGoalAngle) < (Math.PI / 60.0))
                {
                    if (robotAngle < RobotToGoalAngle)
                    {
                        NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, 0));
                        NewResult.AddCommand(CreateTurnLeftCommand(SteeringMotorParams.MAX_LEFT, 0));
                        NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, (int)DelayMS.LONG));
                    }
                    else
                    {
                        NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, 0));
                        NewResult.AddCommand(CreateTurnRightCommand(SteeringMotorParams.MAX_RIGHT, 0));
                        NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, (int)DelayMS.LONG));
                    }
                }
                // Backup and try another way
                if (DriveMotor > (int)DriveMotorParams.STOP)
                {
                    Random random = new Random();
                    int dir = random.Next() % 2;

                    // So what happens now? our forward path is completely blocked. Back up and go around
                    NewResult.AddCommand(CreateSetDriveMotorCommand((int)DriveMotorParams.MIN, 0));
                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, 0));

                    if (dir == 0)
                        NewResult.AddCommand(CreateTurnRightCommand(SteeringMotorParams.MAX_RIGHT, 0));
                    else
                        NewResult.AddCommand(CreateTurnLeftCommand(SteeringMotorParams.MAX_LEFT, 0));

                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, (int)DelayMS.SHORT));
                    NewResult.AddCommand(CreateSetDriveMotorCommand((int)DriveMotorParams.MAX, (int)DelayMS.SHORT));
                }
                
            }
            // Only Right sensor is below critical threshold
            else if ((Sensors[(int)SensorID.LEFT] > CriticalThreshold) && (Sensors[(int)SensorID.MIDDLE] > CriticalThreshold) && (Sensors[(int)SensorID.RIGHT] < CriticalThreshold))
            {
                // Going forward
                if (DriveMotor > (int)DriveMotorParams.STOP)
                {
                    // Do a little jig turn away from this sensor
                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, 0));
                    NewResult.AddCommand(CreateTurnLeftCommand(SteeringMotorParams.MAX_LEFT, 0));
                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, (int)DelayMS.LONG));
                }
            }
            // Only right sensor is below changing threshold
            else if ((Sensors[(int)SensorID.LEFT] > ChangeThreshold) && (Sensors[(int)SensorID.MIDDLE] > ChangeThreshold) && (Sensors[(int)SensorID.RIGHT] < ChangeThreshold))
            {
                // Going forward
                if (DriveMotor > (int)DriveMotorParams.STOP)
                {
                    // Check if we are moving towards the obstacle
                    if ((Sensors[(int)SensorID.RIGHT] - OldSensors[(int)SensorID.RIGHT]) < 0.0f)
                    {
                        // Do a little jig turn away from this sensor
                        NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP,0));
                        NewResult.AddCommand(CreateTurnLeftCommand(SteeringMotorParams.MAX_LEFT,0));
                        NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, (int)DelayMS.LONG));
                    }
                    else
                    {
                        // Head towards the goal if it is on the left side
                        NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, 0));
                        if (robotAngle > RobotToGoalAngle)
                        {
                            if (robotAngle < (RobotToGoalAngle + Math.PI))
                                NewResult.AddCommand(CreateTurnLeftCommand(SteeringMotorParams.MAX_LEFT, 0));
                        }
                        NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, (int)DelayMS.LONG));
                    }
                }
            }
            // Both Left and Middle are below change threshold
            else if ((Sensors[(int)SensorID.LEFT] < ChangeThreshold) && (Sensors[(int)SensorID.MIDDLE] < ChangeThreshold) && (Sensors[(int)SensorID.RIGHT] > ChangeThreshold))
            {
                // Going forward
                if (DriveMotor > (int)DriveMotorParams.STOP)
                {
                    // Do a little jig turn away from this sensor, maybe a little more severe turn than before
                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP,0));
                    NewResult.AddCommand(CreateTurnRightCommand(SteeringMotorParams.MAX_RIGHT,0));
                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, (int)DelayMS.LONG));
                }
            }
            // Both Left and Right are below change threshold
            else if ((Sensors[(int)SensorID.LEFT] < ChangeThreshold) && (Sensors[(int)SensorID.MIDDLE] > ChangeThreshold) && (Sensors[(int)SensorID.RIGHT] < ChangeThreshold))
            {
                // If we are getting close to the obstacle on the right side
                if ((Sensors[(int)SensorID.RIGHT] - OldSensors[(int)SensorID.RIGHT]) < 0.0f)
                {
                    // Do a little jig turn away from this sensor
                    
                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP,0));
                    NewResult.AddCommand(CreateTurnLeftCommand(SteeringMotorParams.MAX_LEFT,0));
                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, (int)DelayMS.SHORT));
                }
                // If we are getting closer to obstacle on left side
                else if ((Sensors[(int)SensorID.LEFT] - OldSensors[(int)SensorID.LEFT]) < 0.0f)
                {
                    // Do a little jig turn away from this sensor
                    
                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP,0));
                    NewResult.AddCommand(CreateTurnRightCommand(SteeringMotorParams.MAX_RIGHT,0));
                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, (int)DelayMS.SHORT));
                }
            }
            // Both Right and Middle are below change threshold
            else if ((Sensors[(int)SensorID.LEFT] > ChangeThreshold) && (Sensors[(int)SensorID.MIDDLE] < ChangeThreshold) && (Sensors[(int)SensorID.RIGHT] < ChangeThreshold))
            {
                // Going forward
                if (DriveMotor > (int)DriveMotorParams.STOP)
                {
                    // Do a little jig turn away from this sensor, maybe a little more severe turn than before
                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP,0));
                    NewResult.AddCommand(CreateTurnLeftCommand(SteeringMotorParams.MAX_LEFT,0));
                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, (int)DelayMS.LONG));
                }
            }
            // All are below critical
            else if ((Sensors[(int)SensorID.LEFT] < CriticalThreshold) && (Sensors[(int)SensorID.MIDDLE] < CriticalThreshold) && (Sensors[(int)SensorID.RIGHT] < CriticalThreshold))
            {
                // Going forward
                if (DriveMotor > (int)DriveMotorParams.STOP)
                {
                    Random random = new Random();
                    int dir = random.Next() % 2;

                    // So what happens now? our forward paths is completely blocked
                    // Do a little jig turn away from this sensor, maybe a little more severe turn than before
                    
                    NewResult.AddCommand(CreateSetDriveMotorCommand((int)DriveMotorParams.MIN,0));
                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP,0));
                    if (dir == 0)
                        NewResult.AddCommand(CreateTurnRightCommand(SteeringMotorParams.MAX_RIGHT,0));
                    else
                        NewResult.AddCommand(CreateTurnLeftCommand(SteeringMotorParams.MAX_LEFT,0));
                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, (int)DelayMS.LONG));
                    NewResult.AddCommand(CreateSetSteerMotorCommand((int)DriveMotorParams.MAX, (int)DelayMS.LONG));
                }
            }
            // All sensors are green! Superman Algorithm in effect!
            else
            {
                // If the robot is going backwards, then go forward again!
                if (DriveMotor < (int)DriveMotorParams.STOP)
                {
                    
                    NewResult.AddCommand(CreateSetDriveMotorCommand((int)DriveMotorParams.MAX,0));
                }

                // Head towards the goal!
                NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, 0));
                if (robotAngle < RobotToGoalAngle)
                {
                    if (robotAngle > (RobotToGoalAngle - Math.PI))
                        NewResult.AddCommand(CreateTurnLeftCommand(SteeringMotorParams.MAX_LEFT, 0));
                    else
                        NewResult.AddCommand(CreateTurnRightCommand(SteeringMotorParams.MAX_RIGHT, 0));
                }
                else if (robotAngle > RobotToGoalAngle)
                {
                    if (robotAngle < (RobotToGoalAngle + Math.PI))
                        NewResult.AddCommand(CreateTurnRightCommand(SteeringMotorParams.MAX_RIGHT, 0));
                    else
                        NewResult.AddCommand(CreateTurnLeftCommand(SteeringMotorParams.MAX_LEFT, 0));
                }
                NewResult.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, (int)DelayMS.LONG));
            }

            // Update the old sensors
            OldSensors = data.sensors;

            if (NewResult.GetNumCommands() == 0)
            {
                NewResult.AddCommand(CreateNoCommand());
                previousResults.Push(NewResult);
            }

            previousResults.Push(NewResult);
            return previousResults.Peek();
        }
    }//class
}//Autobot
